#include "GPS_Handler.h"

const uint8 GPS_NMEA_Config[] = { "$PSRF100,1,38400,8,1,0*3D" };

const uint8 GPS_GGA_Config[] = { GPS_GGA_CONFIGURATION_STRING };

const uint8 GPS_GLL_Config[] = { GPS_GLL_CONFIGURATION_STRING };

const uint8 GPS_GSA_Config[] = { GPS_GSA_CONFIGURATION_STRING };

const uint8 GPS_GSV_Config[] = { GPS_GSV_CONFIGURATION_STRING };

const uint8 GPS_RMC_Config[] = { GPS_RMC_CONFIGURATION_STRING };

const uint8 GPS_VTG_Config[] = { GPS_VTG_CONFIGURATION_STRING };

extern uint8 u8IsRxValid;
extern uint8 u8RxBuffer[];

extern uint8 u8Time_Hours;
extern uint8 u8Time_Min;
extern uint8 u8Time_Sec;

/*
extern uint8 u8LatitudeDeg;
extern uint8 u8LatitudeMin;
extern uint16 u16LatitudeMinDec;
*/

extern uint8 u8Quadrant;

/*
extern uint8 u8LongitudeDeg;
extern uint8 u8LongitudeMin;
extern uint16 u16LongitudeMinDec;
*/

extern uint8 u8PositionFix;
extern uint8 u8Satellites;

extern uint16 u16GPS_Altitude;
extern uint8 u8Speedkm;
extern uint16 u16Course;

extern uUN32 uUN32_latitudeController;
extern uUN32 uUN32_longitudeController;
extern uint8 u8TableLatitude;
extern uUN32 uUN32_latitude;
extern uUN32 uUN32_longitude;
extern uUN32 uUN32_temporal1;
extern uUN16 uUN16_altitudeController;


void vfnGPS_ZeroCoordInit()
{
    uUN32_latitudeController.l=0x04d46fb1;   //Default controller coordinates:
    uUN32_longitudeController.l=0x183cf3f8;; //     "ombligo" del ITESO
    u8TableLatitude=20;                      //************** ADD THIS CALCULUS TO PARSER !!!!!!!!!!!!!!!!!!!!!!!!!
    uUN16_altitudeController.s = 16150;		 //ITESO obligo altitude in meters with one decimal
    //uUN32_latitude.l=0x4C5E2B0;//0x04d48034;        //Delete
    //uUN32_longitude.l=0x183E3646;//0x183ce10d;
}

void vfnGPS_Init (void)
{
    uint8 u8counter;
    
    /* Supply voltage to and enable the GPS module */
    //GPS_VDD_PIN = GPS_VDD_ENABLE;
    //GPS_VDD_PIN_DD = GPIO_OUTPUT;
    
    //GPS_ENABLE_PIN = GPS_ENABLED;
    //GPS_ENABLE_PIN_DD = GPIO_OUTPUT;
    
    /* Wait for GPS module to start WAIT = 8ms*/
    vfnGPS_WatchdogInit ();
    
    vfnMTIM_SetStatus (MTIM_ACTIVE);
    
    for(u8counter = 0; u8counter < GPS_INITIALIZATION_WAIT; u8counter++)
    {
        while(!u8MTIM_CheckOvFlag());   
        vfnMTIM_Reset();
    }
    
    vfnMTIM_SetStatus (MTIM_INACTIVE);
    
    /* Begin GPS configuration command transmissions */
    
    /* Configure GPS for NMEA output at 9600, 8 bit, 1 stop bit, no parity */
    for(u8counter = 0; GPS_NMEA_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_NMEA_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    vfnSCI_SetBaudRate (SCI_38400BAUD);
    
    __RESET_WATCHDOG();
    
    vfnMTIM_SetStatus (MTIM_ACTIVE);
    
    for(u8counter = 0; u8counter < 10; u8counter++)
    {
        while(!u8MTIM_CheckOvFlag());   
        vfnMTIM_Reset();
    }
    
    vfnMTIM_SetStatus (MTIM_INACTIVE);
    
    
    /* Configure GGA query at a 2 second rate, checksum enabled */
    for(u8counter = 0; GPS_GGA_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_GGA_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();

    /* Configure GGL query as disabled */
    for(u8counter = 0; GPS_GLL_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_GLL_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();
    
    /* Configure GSA query as disabled */
    for(u8counter = 0; GPS_GSA_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_GSA_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();
    
    /* Configure GSV query as disabled */
    for(u8counter = 0; GPS_GSV_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_GSV_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();
    
    /* Configure RMC query as disabled  */
    for(u8counter = 0; GPS_RMC_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_RMC_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();
    
    /* Configure VTG query at a 2 second rate, checksum enabled */
    for(u8counter = 0; GPS_VTG_Config[u8counter] != 0; u8counter++)
    {
        vfnSCI_SendByte (GPS_VTG_Config[u8counter]);
        while(!u8SCI_TxBufferEmpty());
    }
    vfnGPS_SendTermination();
    
    __RESET_WATCHDOG();
    
    return;
}

void vfnGPS_SendTermination (void)
{
    vfnSCI_SendByte(13);
    while(!u8SCI_TxBufferEmpty());
    vfnSCI_SendByte(10);
    while(!u8SCI_TxBufferEmpty());
    
}

void vfnGPS_WatchdogInit (void)
{
    vfnMTIM_PrescalerSelect (MTIM_PRESCALER_256);
    vfnMTIM_SetModulo (0xFF);
    vfnMTIM_SetStatus (MTIM_INACTIVE);
    vfnMTIM_Reset ();
}

void vfnGPS_ParseBuffer (void)
{
    uint8 u8parseCounter;
    uint8 u8commaCounter;
    
    if (u8IsRxValid & GPS_PARSE_GGA)
    {
        u8Time_Hours = u8RxBuffer[0];
        u8Time_Hours *= 10;
        u8Time_Hours += u8RxBuffer[1];
        
        u8Time_Min = u8RxBuffer[2];
        u8Time_Min *= 10;
        u8Time_Min += u8RxBuffer[3];
        
        u8Time_Sec = u8RxBuffer[4];
        u8Time_Sec *= 10;
        u8Time_Sec += u8RxBuffer[5];
    
        
        uUN32_latitude.l = 0;
        uUN32_temporal1.l = 0;
        
        uUN32_latitude.bytes.b2 = (u8RxBuffer[11] * 10);
        uUN32_latitude.shorts.s1 += u8RxBuffer[12];
        uUN32_latitude.shorts.s1 *= 60;
        
        uUN32_latitude.shorts.s1 += (u8RxBuffer[13] * 10);
        uUN32_latitude.shorts.s1 += u8RxBuffer[14];
        
        uUN32_temporal1.shorts.s1 = u8RxBuffer[16];
        uUN32_temporal1.shorts.s1 *= 10;
        uUN32_temporal1.shorts.s1 += u8RxBuffer[17];
        uUN32_temporal1.shorts.s1 *= 10;
        uUN32_temporal1.shorts.s1 += u8RxBuffer[18];
        uUN32_temporal1.shorts.s1 *= 10; 
        uUN32_temporal1.shorts.s1 += u8RxBuffer[19];
        
        uUN32_temporal1.l /= 10000;
        
        uUN32_latitude.shorts.s0 = uUN32_temporal1.shorts.s0; 
        
        /*
        u8LatitudeDeg = u8RxBuffer[11];
        u8LatitudeDeg *= 10;
        u8LatitudeDeg += u8RxBuffer[12];
        
        u8LatitudeMin = u8RxBuffer[13];
        u8LatitudeMin *= 10;
        u8LatitudeMin += u8RxBuffer[14];
        
        u16LatitudeMinDec = u8RxBuffer[16];
        u16LatitudeMinDec *= 10;
        u16LatitudeMinDec += u8RxBuffer[17];
        u16LatitudeMinDec *= 10;
        u16LatitudeMinDec += u8RxBuffer[18];
        u16LatitudeMinDec *= 10;
        u16LatitudeMinDec += u8RxBuffer[19];
        */
        
        u8Quadrant = 0x00;
        
        if (u8RxBuffer[21] == 0x1E) // 'N'
        {
            u8Quadrant |= GPS_LAT_NORTH;
        }
        else
        {
            u8Quadrant |= GPS_LAT_SOUTH;
        }
        
        uUN32_longitude.l = 0;
        uUN32_temporal1.l = 0;
        
        uUN32_longitude.bytes.b2 = (u8RxBuffer[23] * 10);
        uUN32_longitude.shorts.s1 += u8RxBuffer[24];
        uUN32_longitude.shorts.s1 *= 10;
        uUN32_longitude.shorts.s1 += u8RxBuffer[25];
        uUN32_longitude.shorts.s1 *= 60;
        
        uUN32_longitude.shorts.s1 += (u8RxBuffer[26] * 10);
        uUN32_longitude.shorts.s1 += u8RxBuffer[27];
        
        uUN32_temporal1.shorts.s1 = u8RxBuffer[29];
        uUN32_temporal1.shorts.s1 *= 10;
        uUN32_temporal1.shorts.s1 += u8RxBuffer[30];
        uUN32_temporal1.shorts.s1 *= 10;
        uUN32_temporal1.shorts.s1 += u8RxBuffer[31];
        uUN32_temporal1.shorts.s1 *= 10; 
        uUN32_temporal1.shorts.s1 += u8RxBuffer[32];
        
        uUN32_temporal1.l /= 10000;
        
        uUN32_longitude.shorts.s0 = uUN32_temporal1.shorts.s0; 
        
        /* 
        u8LongitudeDeg = u8RxBuffer[23];
        u8LongitudeDeg *= 10;
        u8LongitudeDeg += u8RxBuffer[24];
        u8LongitudeDeg *= 10;
        u8LongitudeDeg += u8RxBuffer[25];
        
        u8LongitudeMin = u8RxBuffer[26];
        u8LongitudeMin *= 10;
        u8LongitudeMin += u8RxBuffer[27];
        
        u16LongitudeMinDec = u8RxBuffer[29];
        u16LongitudeMinDec *= 10;
        u16LongitudeMinDec += u8RxBuffer[30];
        u16LongitudeMinDec *= 10;
        u16LongitudeMinDec += u8RxBuffer[31];
        u16LongitudeMinDec *= 10;
        u16LongitudeMinDec += u8RxBuffer[32];
        */
        
        if (u8RxBuffer[34] == 0x27) // 'W'
        {
            u8Quadrant |= GPS_LONG_WEST;
        }
        else
        {
            u8Quadrant |= GPS_LONG_EAST;
        }
        
        u8PositionFix = u8RxBuffer[36];
        
        u8Satellites = u8RxBuffer[38];
        u8Satellites *= 10;
        u8Satellites += u8RxBuffer[39];

        // The program looks for the altitude, and in the process, ignores
        // horizontal dilution of precision
        u8parseCounter = 41;
        while(u8RxBuffer[u8parseCounter] != 0xFC) // ','
        {
            u8parseCounter++;
        }
        
        // point to the beginning of the altitude section
        u8parseCounter++;
        
        u16GPS_Altitude = 0;
        
        while(u8RxBuffer[u8parseCounter] != 0xFE) // '.'
        {
            u16GPS_Altitude += u8RxBuffer[u8parseCounter];
            u16GPS_Altitude *= 10;
            u8parseCounter++;
        }
        
        u8parseCounter++;
        
        u16GPS_Altitude += u8RxBuffer[u8parseCounter];

    }
    else if (u8IsRxValid & GPS_PARSE_VTG)
    {
        u8parseCounter = 0;
        u8Speedkm = 0;
        u16Course = 0;
        while(u8RxBuffer[u8parseCounter] != 0xFE) // '.'
        {
            u16Course *= 10;
            u16Course += u8RxBuffer[u8parseCounter];
            u8parseCounter++;       
        }
        
        u8commaCounter = 6;
        while(u8commaCounter)
        {
            if(u8RxBuffer[u8parseCounter] == 0xFC) // ','
            {
                u8commaCounter--;
            }
            u8parseCounter++;
        }
        
        while(u8RxBuffer[u8parseCounter] != 0xFE) // '.'
        {
            u8Speedkm *= 10;
            u8Speedkm += u8RxBuffer[u8parseCounter];
            u8parseCounter++;       
        }
    }
}


